//
// File: qrsolve.cpp
//
// MATLAB Coder version            : 4.0
// C/C++ source code generated on  : 18-Dec-2020 16:50:24
//

// Include Files
#include <cmath>
#include "rt_nonfinite.h"
#include "sea_parameter_cal.h"
#include "qrsolve.h"
#include "sqrt.h"
#include "sea_parameter_cal_emxutil.h"
#include "xswap.h"
#include "ixamax.h"

// Function Declarations
static void LSQFromQR(const emxArray_real_T *A, const emxArray_int32_T *jpvt,
                      int rankA, emxArray_real_T *Y);
static int rankFromQR(const emxArray_real_T *A);

// Function Definitions

//
// Arguments    : const emxArray_real_T *A
//                const emxArray_int32_T *jpvt
//                int rankA
//                emxArray_real_T *Y
// Return Type  : void
//
static void LSQFromQR(const emxArray_real_T *A, const emxArray_int32_T *jpvt,
                      int rankA, emxArray_real_T *Y)
{
  int unnamed_idx_0;
  int i3;
  unnamed_idx_0 = A->size[1];
  i3 = Y->size[0];
  Y->size[0] = unnamed_idx_0;
  emxEnsureCapacity_real_T(Y, i3);
  for (i3 = 0; i3 < unnamed_idx_0; i3++) {
    Y->data[i3] = 0.0;
  }

  unnamed_idx_0 = 1;
  while (unnamed_idx_0 <= rankA) {
    Y->data[jpvt->data[0] - 1] = 6.2831853071795862;
    unnamed_idx_0 = 2;
  }

  unnamed_idx_0 = rankA;
  while (unnamed_idx_0 > 0) {
    Y->data[jpvt->data[0] - 1] /= A->data[0];
    unnamed_idx_0 = 0;
  }
}

//
// Arguments    : const emxArray_real_T *A
// Return Type  : int
//
static int rankFromQR(const emxArray_real_T *A)
{
  int r;
  double tol;
  r = 0;
  tol = (double)A->size[1] * std::abs(A->data[0]) * 2.2204460492503131E-16;
  if (!(std::abs(A->data[0]) <= tol)) {
    r = 1;
  }

  return r;
}

//
// Arguments    : const emxArray_real_T *A
//                emxArray_real_T *Y
// Return Type  : void
//
void qrsolve(const emxArray_real_T *A, emxArray_real_T *Y)
{
  emxArray_real_T *b_A;
  int itemp;
  int yk;
  emxArray_int32_T *jpvt;
  int n;
  emxArray_real_T *vn1;
  emxArray_real_T *vn2;
  double TOL3Z;
  double atmp;
  double temp2;
  emxInit_real_T1(&b_A, 2);
  itemp = b_A->size[0] * b_A->size[1];
  b_A->size[0] = 1;
  b_A->size[1] = A->size[1];
  emxEnsureCapacity_real_T1(b_A, itemp);
  yk = A->size[0] * A->size[1];
  for (itemp = 0; itemp < yk; itemp++) {
    b_A->data[itemp] = A->data[itemp];
  }

  emxInit_int32_T1(&jpvt, 2);
  n = A->size[1];
  itemp = jpvt->size[0] * jpvt->size[1];
  jpvt->size[0] = 1;
  jpvt->size[1] = A->size[1];
  emxEnsureCapacity_int32_T1(jpvt, itemp);
  jpvt->data[0] = 1;
  yk = 1;
  for (itemp = 2; itemp <= A->size[1]; itemp++) {
    yk++;
    jpvt->data[itemp - 1] = yk;
  }

  emxInit_real_T(&vn1, 1);
  emxInit_real_T(&vn2, 1);
  TOL3Z = 2.2204460492503131E-16;
  b_sqrt(&TOL3Z);
  yk = A->size[1];
  itemp = vn1->size[0];
  vn1->size[0] = yk;
  emxEnsureCapacity_real_T(vn1, itemp);
  itemp = vn2->size[0];
  vn2->size[0] = vn1->size[0];
  emxEnsureCapacity_real_T(vn2, itemp);
  itemp = 0;
  for (yk = 0; yk < n; yk++) {
    vn1->data[yk] = std::abs(A->data[itemp]);
    vn2->data[yk] = vn1->data[yk];
    itemp++;
  }

  yk = ixamax(n, vn1) - 1;
  if (yk + 1 != 1) {
    xswap(b_A, yk + 1, 1);
    itemp = jpvt->data[yk];
    jpvt->data[yk] = jpvt->data[0];
    jpvt->data[0] = itemp;
    vn1->data[yk] = vn1->data[0];
    vn2->data[yk] = vn2->data[0];
  }

  atmp = b_A->data[0];
  b_A->data[0] = 1.0;
  b_A->data[0] = atmp;
  for (yk = 1; yk < n; yk++) {
    if (vn1->data[yk] != 0.0) {
      atmp = std::abs(b_A->data[b_A->size[0] * yk]) / vn1->data[yk];
      atmp = 1.0 - atmp * atmp;
      if (atmp < 0.0) {
        atmp = 0.0;
      }

      temp2 = vn1->data[yk] / vn2->data[yk];
      temp2 = atmp * (temp2 * temp2);
      if (temp2 <= TOL3Z) {
        vn1->data[yk] = 0.0;
        vn2->data[yk] = 0.0;
      } else {
        b_sqrt(&atmp);
        vn1->data[yk] *= atmp;
      }
    }
  }

  emxFree_real_T(&vn2);
  emxFree_real_T(&vn1);
  yk = rankFromQR(b_A);
  LSQFromQR(b_A, jpvt, yk, Y);
  emxFree_int32_T(&jpvt);
  emxFree_real_T(&b_A);
}

//
// File trailer for qrsolve.cpp
//
// [EOF]
//
